#include <Servo.h>
#include <EEPROM.h>

///////////////////////////////////////////////////
// Credit jmknapp, for this block of code
// defines for setting and clearing register bits
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
///////////////////////////////////////////////////


/*
 *  ServoMember - struct used to describe a servo that has been
 *                modified to work as a servo providing force feed back.
 */
struct ServoMember{
  Servo servo;
  byte feedbackPin;
  
  int destPos;
  int initPos;
  int elapsed;
  
  byte lastFeedBack;
  int resistance;
  
  int tolerance;
  int m;
  float w;
  float rotSpeed; // milliseconds / degree
};



// NOTE: 0 is full left, 90 deg is center, 180 is full right
unsigned long oldTime; 
int totalTime = 0;
boolean calibrated = false;

boolean loadCalib = true;
int addr = 0;

struct ServoMember servo[1];

void setup(){
  
  /////////////////////////////////////
  // set prescale to 16
  // thereby improving the sample rate,
  // and decreasing resolution.
  sbi(ADCSRA,ADPS2) ;
  cbi(ADCSRA,ADPS1) ;
  cbi(ADCSRA,ADPS0) ;
  /////////////////////////////////////
  
  Serial.begin(9600);
  
  //servo = (struct ServoMember*)malloc(sizeof(struct ServoMember));
  servo[0] = createServo(2, 0, 90, 4);
 // Serial.print("pin = "); Serial.println(servo[0].feedbackPin, DEC);
}

void loop(){
    calibrateServos(servo, 1);
    
    if(Serial.available()>0){ 
      int value = 0;

      while(Serial.available() > 0) {
         int inByte = Serial.read()-32;
         value += inByte;
      }
    
      Serial.print("YOUR IN: ");
      Serial.println(value, DEC);
      
      setServoPos(servo, value);
    }
    
    for(int i = 0; i < 173; i++){
      setServoPos(servo, i);
      updateServoPositions(servo, 1);
      delay(50);
      int fb = estPositionFB(servo);
      int a  = i;
      
      /*Serial.print("Estimated angle(feedback): ");
      Serial.print(fb, DEC);
      Serial.print(" Estimated angle(time): ");
      Serial.print(estPositionTime(servo), DEC);
      Serial.print(" Actual angle: ");
      Serial.println(i, DEC);*/
      
      Serial.println(analogRead(0), DEC);
      
      if(servo->resistance != 0){
        Serial.print(servo->resistance, DEC);
        Serial.print(", ");
        Serial.print(fb, DEC);
        Serial.print(", ");
        Serial.println(a, DEC);
        ///break;
        delay(1000);
      }
      
      //if(i == 172) i = 0;
    }
    delay(1000 * 60);
}

struct ServoMember createServo(byte ctrlPin, byte fbPin, int dest, int tolerance){
  struct ServoMember out;
  out.servo.attach(ctrlPin);
  setFeedbackPin(&out, fbPin);
  out.destPos = dest;
  out.tolerance = tolerance;
  
  return out;
}

void setFeedbackPin(struct ServoMember *servo, byte pin){
  servo->feedbackPin = pin;
}

void setServoPos(struct ServoMember *servo, int angle){
 int est = estPositionFB(servo);
 servo->initPos =  est >= 0 ? est : 0;
 servo->destPos = angle;
 servo->elapsed = 0;
}

void calibrateServos(struct ServoMember *servos, int count){
  
  // first check to see if we have calibration data
  // for the servos saved.
  if(loadCalib){
    // check to see if the word 'TAC' is saved at the
    // provided address, if so. Then we have valid data.
    if(loadInt(addr) == 0x00544143){
      int a = addr + 4; //data after the header
      for(int i = 0; i < count; i++){
        float   w = loadFloat(a + (i*12));
        int     m = loadInt(a + (i*12) + 4);
        float rot = loadFloat(a + (i*12) + 8);
        servos[i].m = m;
        servos[i].w = w;
        servos[i].rotSpeed = rot;
      }
      calibrated = true;
    }
  }
  
  if(!calibrated){
    
    int firstTime = 0;
    int lastTime  = 0;
    int oldInput  = 0;
    int a = addr + 4; //data after the header
    
    if(loadCalib){
      // write the tag 'TAC'
      int tag = 5521731;
      saveInt(&tag, addr);
      delay(10000);
    }
    
    for(int i = 0; i < count; i++){
      servos[i].servo.write(0);
      delay(2000);                     // make sure we are at 0
      servos[i].m = sampleAnalog(servos[i].feedbackPin); // sample the minimum

      // calibrates by finding the minimum and max values
      // that the servo reaches. Then calcs the slope.
      int mn, mx;

      firstTime = millis(); // get the time at beginning of calibration
      mn = sampleAnalog(servos[i].feedbackPin);
      
      while(oldInput != sampleAnalog(servos[i].feedbackPin)){
            servos[i].servo.write(173);
            delay(100);
            oldInput = sampleAnalog(servos[i].feedbackPin);
            delay(100);
      }
      lastTime = millis(); // get the time when calibration finished.
      
      mx = sampleAnalog(servos[i].feedbackPin);
      // calculate slope
      servos[i].w = (mx - mn) / 173.0f;
      servos[i].rotSpeed = (lastTime - firstTime) / 173.0f;
      
      // write the servo's calibration data
      if(loadCalib){
        saveFloat(&servos[i].w,        a + (i * 12));
        saveInt(  &servos[i].m,        a + (i * 12) + 4);
        saveFloat(&servos[i].rotSpeed, a + (i * 12) + 4);
      }
      
      Serial.println("f(a) = w * a + m");
      Serial.print("m = "); Serial.println(servos[i].m, DEC);
      Serial.print("w = "); Serial.println(servos[i].w, DEC);
      Serial.print("Speed = ");Serial.println(servos[i].rotSpeed, DEC);
      Serial.print("Total time = ");Serial.println(lastTime - firstTime, DEC);
      servos[i].servo.write(0);
      delay(1000);
    }
    calibrated = true;
  }
}

int estFeedback(struct ServoMember *servo){
  // was using servo->destPos instead of the estimation
 return (int)(servo->w * estPositionTime(servo)) + servo->m; 
}

/* accepts a pointer to a servo and return's the expected position
   based upon the feedback retrieved for that servo. */
int estPositionFB(struct ServoMember *servo){
 int est = (int)((servo->lastFeedBack - servo->m) / servo->w);
 return est >= 0 ? est : 0;
}

/* accepts a pointer to a servo and return's the expected position
   based upon the elapsed time that the servo has accrued while
   rotating.  */
int estPositionTime(struct ServoMember *servo){
  // get the normalized direction of rotation of the servo.
  int delta = servo->destPos - servo->initPos;
  
  if(delta!=0) // we don't want to crash everything...
    delta /= abs(delta);
    
  // calctulate the expected angle based on time
  return (delta * servo->elapsed * servo->rotSpeed) + servo->initPos;
}

// Samples the analog output from the servo
// then selects the two closest samples and
// averages them.
int sampleAnalog(int pin){
  int x[3];
  int d[3];
  int less[2];
  
  for(int i = 0; i < 3; i++){
    x[i] = analogRead(pin)/4; // may want to do div after loop
    //delay(10); 
  }
   d[0] = abs(x[0] - x[1]);
   d[1] = abs(x[1] - x[2]);
   d[2] = abs(x[0] - x[2]);
   
   // find the smallest delta
   int smallest = 99999;
   int smIndex = 0;
   
   for(int i = 0; i < 3; i++)
     if(d[i] <= smallest){
       smIndex = i;
       smallest = d[i]; 
     }
     
  // then average the two samples that
  // contibuted to the delta and return them.
  switch(smIndex){
    case 0:
     return (x[0] + x[1]) / 2;
    break;
    case 1:
     return (x[1] + x[2]) / 2;
    break;
    case 2:
     return (x[0] + x[2]) / 2;
    break;
  }
}

void updateServoPositions(struct ServoMember *servos, int count){
  int currentTime = millis();
  int elapsedTime = (int)(currentTime - oldTime);
  
  for(int i = 0; i < count; i++){
    int pos = servos[i].destPos;
    servos[i].servo.write(pos);
    
    if(servos[i].feedbackPin >= 0){
      // retrieve the latest feedback data.
      servos[i].lastFeedBack = sampleAnalog(servos[i].feedbackPin);
      
      // check to see if the servo is approximatly at the destination position.
      // if so, don't increment the elapsed time.
      byte estAngle = estPositionFB(&servos[i]);
      if((estAngle - servos[i].destPos) > servos[i].tolerance){
        servos[i].elapsed += elapsedTime;
      }
      
      // calculate the difference between the predicted position based upon time
      // and the position extrapolated from the feedback data. Then figure out if the
      // difference is significant enough to report it as resistance.
      int estAngleTime = estPositionTime(&servos[i]);
      int diff = estAngle - estAngleTime;
      servos[i].resistance = abs(diff) > servos[i].tolerance ? diff : 0;
      
    }
    else
      servos[i].elapsed += elapsedTime;
  }
  
  oldTime = currentTime; 
}

////////////////////////////////////////////////
//  functions saveInt() and saveFloat() are   //
//  used for saving the calibration of servos //
////////////////////////////////////////////////
void saveInt(int* i, int offset){
  
  byte* b = (byte*)i;
  
  for(int j = 3; j >= 0; j--){
   EEPROM.write(offset + j, *(b + j));
  }
}

void saveFloat(float* f, int offset){
  
  byte* b = (byte*)f;
  
  for(int j = 3; j >= 0; j--){
   EEPROM.write(offset + j, *(b + j));
  }
}

int loadInt(int address){
  byte* i = (byte*)malloc(4);
  int out;
  for(int j = 3; j >= 0; j--){
   byte b = EEPROM.read(address + j);
   *(i + j) = b;
  }
  
  out = *((int*)i);
  free(i);
  return out;
}

float loadFloat(int address){
  byte* i = (byte*)malloc(4);
  float out;
  for(int j = 3; j >= 0; j--){
   byte b = EEPROM.read(address + j);
   *(i + j) = b;
  }
  
  out = *((float*)i);
  free(i);
  return out;
}
